%% setup.m
% UAV Nonlinear Simulation setup
% This script will setup the nonlinear simulation (UAV_NL.mdl) and call
% trim and linearization routines. Select the desired aircraft here in this
% script, via the "UAV_config()" function call.
% Calls: 
%        trim_uav.m
%        linearize_UAV.m
clear all
clc
data_aero_vtp;
%% Set aircraft initial conditions 
% Note: these are NOT the trim condition targets. If the trim fails to
% coverge, try using different initial conditions. Also note that the trim
% function will overwrite these initial conditions with the trimmed
% conditions.
% Set the initial model inputs
TrimCondition.Inputs.da   = 0; % rad
TrimCondition.Inputs.de   = -1.1;     % rad
TrimCondition.Inputs.dr   = 0;     % rad
TrimCondition.Inputs.df   = 0;    % rad
TrimCondition.Inputs.dT   = 0.4;     % nd, 0 to 1
% Set initial state values
TrimCondition.XIni      = Xme_0;   % Initial Position in Inertial Frame [Xe Ye Ze], [m]
TrimCondition.VbIni     = Uvw0; % Initial Body Frame velocities [u v w], [m/s]
TrimCondition.EulerIni  = euler_0; % Initial Euler orientation [roll,pitch,yaw] [rad]
TrimCondition.pqrIni    = pqr0;      % Initial Body Frame rotation rates [p q r], [rad/s]
%% Trim aircraft to a specific flight condition
% Set the trim targets here. See trim_UAV for complete list of target
% variables. Sideslip angle (beta), flight path angle (gamma) and flap
% setting default to zero. If a fixed control input setting is desired,
% specify as a target.
speeds=20:10:40;
alt=200:100:400;
chiso=1;
for index_v=1:length(speeds),
    for index_h=1:length(alt),
TrimCondition.target = struct('V',speeds(index_v),'gamma',0,'h',-alt(index_h)); % level flight at different airspeed(m/s) and altitude(m)
% TrimCondition.target = struct('V',37,'gamma',0,'h',-400);
% TrimCondition.target = struct('V',37,'h',-300)
%TrimCondition.target.da = 0;
%TrimCondition.target.dr = 0;
%TrimCondition.target.df = 0;
%TrimCondition.target.dT = 0.5;
% TrimCondition.target = struct('V',37.1,'gamma',7.5/180*pi); % level climb, (m/s, rad)
% TrimCondition.target = struct('V',17,'gamma',0,'psidot',20/180*pi); % level turn, (m/s, rad, rad/sec)
% TrimCondition.target = struct('V',17,'gamma',5/180*pi,'psidot',20/180*pi); % climbing turn, (m/s, rad, rad/sec)
% TrimCondition.target = struct('V',17,'gamma',0,'beta',5/180*pi); % level steady heading sideslip, (m/s, rad, rad)
% Find the trim solution
 [TrimCondition,OperatingPoint] = trim_uav(TrimCondition,'VTP_airframe');
% Linearize about the operating point
 [longmod,spmod,latmod,linmodel]=linearize_vtp(OperatingPoint);
%Save matrices in files
save(strcat('Matrices_wl',num2str(chiso),'.mat'),'longmod','spmod','latmod','linmodel');
% save(strcat('Matrices_wl.mat'),'longmod','spmod','latmod','linmodel');
chiso=chiso+1;
% save(strcat('Matrices_wl',num2str(index),'.mat'),'spmod');
    end
end
%% Linearize about the operating point
% [longmod,spmod,latmod,linmodel]=linearize_vtp(OperatingPoint);
% save('VTP_ssmodes_wl(index).mat','longmod','spmod','latmod','linmodel');
%initial vectors
% Hdot0_wl=0;
% xinco_wl=[OperatingPoint.op_point.States(1).x;OperatingPoint.op_point.States(2).x;OperatingPoint.op_point.States(3).x;...
%     OperatingPoint.op_point.States(4).x(1);OperatingPoint.op_point.States(4).x(2);OperatingPoint.op_point.States(4).x(3);Hdot0_wl];
% xinco1_wl=[OperatingPoint.op_point.States(1).x;OperatingPoint.op_point.States(2).x;OperatingPoint.op_point.States(3).x;...
%     OperatingPoint.op_point.States(4).x(1);OperatingPoint.op_point.States(4).x(2);-OperatingPoint.op_point.States(4).x(3);Hdot0_wl];
% uinco_wl=[OperatingPoint.op_point.Inputs(1).u,OperatingPoint.op_point.Inputs(2).u,OperatingPoint.op_point.Inputs(3).u,OperatingPoint.op_point.Inputs(4).u];
% dTinco_wl=OperatingPoint.op_point.Inputs(5).u;
% V0_wl=OperatingPoint.op_report.outputs(19).y;
% save('inco_wl.mat','xinco_wl','xinco1_wl','uinco_wl','dTinco_wl','V0_wl');



